#include <stm32f10x.h>
/******************************mylib**********************************/
#include "parameter.h"
//#include "GY_521.h"
#include "attitudeAlgorithm.h"
#include "Peripheral.h"
//#include "Communication.h"
#include "SPI.h"
#include "Controller.h"
#include "HIL.h"
#include "MPU9250.h"
#include "Filter.h"
#include "SampleSensorData.h"
#include "led.h"
#include "publicData.h"
/**************************outsideSource******************************/
#include "sys.h"
#include "delay.h"
#include "usart.h"
//#include "mpu6050.h"
//#include "inv_mpu.h"
//#include "inv_mpu_dmp_motion_driver.h"
/*****************************section.old*******************************/

/*****************************PID参数***********************************///全部调至微微幌动时可以适当在跳大P
//const u16 a1 __attribute__((at(0x08007800+26)))=28;
//const u16 a3 __attribute__((at(0x08007800+24)))=1;
//const u16 a5 __attribute__((at(0x08007800+22)))=1500;

//const u16 b1 __attribute__((at(0x08007800+20)))=28;
//const u16 b3 __attribute__((at(0x08007800+18)))=1;
//const u16 b5 __attribute__((at(0x08007800+16)))=1500;

//const u16 c1 __attribute__((at(0x08007800+14)))=28;
//const u16 c3 __attribute__((at(0x08007800+12)))=1;
//const u16 c5 __attribute__((at(0x08007800+10)))=1500;

void TIM6_IRQHandler(void)//每个Ts时间间隔都会产生一次控制指令
{
    if(TIM6->SR & 0X0001) //溢出中断
    {
        get_contral_input_DATE();
				
				Read_MPU9250(&MPU9250_sensorData);
				getMPU9250CalibratedData(&MPU9250_sensorData);
				accelerateData.X = MPU9250_sensorData.Accel_DataX_cal;
				accelerateData.Y = MPU9250_sensorData.Accel_DataY_cal;
				accelerateData.Z = MPU9250_sensorData.Accel_DataZ_cal;

				gyroscopeData.X = MPU9250_sensorData.Gyro_DataX_cal;
				gyroscopeData.Y = MPU9250_sensorData.Gyro_DataY_cal;
				gyroscopeData.Z = MPU9250_sensorData.Gyro_DataZ_cal;

				magnetometerData.X = MPU9250_sensorData.Mag_DataX_cal;
				magnetometerData.Y = MPU9250_sensorData.Mag_DataY_cal;
				magnetometerData.Z = MPU9250_sensorData.Mag_DataZ_cal;
				
		PID_Controller();
    }
    TIM6->SR &= ~(1 << 0); //清除中断标志位
}

/**********************************************************************************
函数功能：	主函数
					PA0接油门通道  	TIM2
					PA6接偏航通道		TIM3
					PB6接俯仰通道		TIM4
					PA1接横滚通道		TIM5
**********************************************************************************/
int main()
{	
	Stm32_Clock_Init(9);				//系统时钟设置
	delay_init(72);							//延时初始化	
	delay_ms(500);
	LED_Init();	//LED_Init
	LED0=0;
	LED1=0;
	uart_init(72, 115200);			//串口1初始化为500000
	usart2_init(36, 115200);		//串口2初始化115200  接收调试参数
	printf("# Initing......\n");		

	TIM2_InputPWM_Init();				//初始化PWM检测
	TIM3_InputPWM_Init();				//初始化PWM检测
	TIM4_InputPWM_Init();				//初始化PWM检测
	TIM5_InputPWM_Init();				//初始化PWM检测
	TIM8_Output4PWM_Init();			//初始化PWM四路输出
	
#if USE_SPI_OR_IIC
	IIC_Init();									//初始化软件IIC
#else
	SPI2_Init();
	SPI2_SetSpeed(6);
	Init_SPICS();
#endif

	delay_ms(1);
	printf("# Initing MPU9250 \r\n");
	Init_MPU9250();
	printf("# MPU92500 Initied # \r\n");
	
//	InitPXISensor();
	
	SyncParameterFlashToPar();	//FLASH->par
	Modify_PID();								//将PID常量赋值给对应PID变量par->参数变量
	RunState = LOCKED;					//设置运行状态标记
	
		if(Position_YAW_P==0)				//校准陀螺零偏
		{
			delay_ms(500);
			delay_ms(500);
			delay_ms(500);
			delay_ms(500);
			calMPU9250GyroBiasXYZ(1000);
		}
	printf("# Initied # \r\n");	
	LED0=1;
	LED1=1;
	TIM6_Int_Init();//初始化定时中断,最后初始化
	while(1)
	{	
//		LED0=0;
//		LED1=1;
//		delay_ms(300);
//		LED0=1;
//		LED1=0;
//		delay_ms(1000);
//    printf("WIA is %d\r\n",	AK8963_Read_RegSPI(WIA));
//			Read_MPU9250(&MPU9250_sensorData);
//			printf("# Accel Data is \r\n");
			display_3AxisData(MPU9250_sensorData.Accel_DataX_raw,MPU9250_sensorData.Accel_DataY_raw,MPU9250_sensorData.Accel_DataZ_raw);
//			printf("# Gyro Data is \r\n");
			display_3AxisData(MPU9250_sensorData.Gyro_DataX_raw,MPU9250_sensorData.Gyro_DataY_raw,MPU9250_sensorData.Gyro_DataZ_raw);
//			printf("# Mag Data is \r\n");
			display_3AxisData(MPU9250_sensorData.Mag_DataX_raw,MPU9250_sensorData.Mag_DataY_raw,MPU9250_sensorData.Mag_DataZ_raw);
//			printf("# Attitude is \r\n");
//			display_3AxisData(attitudeData.pitch,attitudeData.roll,attitudeData.yaw);
		
			if(RunState == LOCKED){PID_WriteAndRead();get_contral_input_DATE();}
			if(RunState == RUNNING){/*printf("%f \r\n", gyroscopeData.Z);*/} //feedBack_roll//gyroscopeData.X
			/******************************电调解锁上锁程序段down******************************/
			if(conPPM < 1220 && conYAW < 1220 && conPPM > 1000)
			{
					//delay_ms(1000);
					TIM6->CR1 &= 0; //停止计时器 即电机停止
					TIM6->DIER &= 0;
					TIM8->CCR1 = 1000 - 1;
					TIM8->CCR2 = 1000 - 1;
					TIM8->CCR3 = 1000 - 1;
					TIM8->CCR4 = 1000 - 1;
					con1 = 1000;
					con2 = 1000;
					con3 = 1000;
					con4 = 1000;
					//上锁后积分值归零（源于网络上的说法）
					In_Diff_pitch_sum = 0;
					In_Diff_roll_sum = 0;
					In_Diff_yaw_sum = 0;
					In_Diff_pitch_old = 0;
					In_Diff_roll_old = 0;
					In_Diff_yaw_old = 0;
					In_Diff_pitch_rate_old = 0;
					In_Diff_roll_rate_old = 0;
					In_Diff_yaw_rate_old = 0;

					Out_Diff_pitch_sum = 0;
					Out_Diff_roll_sum = 0;
					
					OutofPID_pitch=0;
					OutofPID_roll=0;
					ButterworthFilter_Init();
					//历史性参数在上锁后清零（实时性参数不用）
					RunState = LOCKED; //设置运行状态为上锁
			}
			if(conPPM < 1220 && conYAW > 1780)
			{
					delay_ms(1000);
					//开始计时器 即电机启动
					TIM6->DIER |= 1;
					TIM6->CR1 |= 1;
					RunState = RUNNING; //设置运行状态为运行
			}
			/******************************电调解锁上锁程序段up******************************/
			//debug code start
			//temp=MPU_Get_Temperature();	//得到温度值
			//MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//得到加速度传感器数据
			//MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//得到陀螺仪数据
			//mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
			//mpu6050_send_data(roll,pitch,yaw,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
			//usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));//发送姿态数据
			//usart1_report_contral_DATE();//上传遥控数据
			//print_con_date_on_PC();
			//print_PWM_on_PC();
			//debug code end
			//void KF();//得到滤波后的欧拉角值 以及角速率
	}
}
/*
*添加一个启动自己计算补偿的算法
*添加平方根控制
*俯仰横滚通道加输出限幅，这样有助于提高外环P
*/
